﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Kinect;
using Kinect.Gesture;

namespace Kinect.Toolbox
{
    public class ZoomGestureEventArgs : EventArgs
    {

        public Vector2Struct StartLeftHand;
        public Vector2Struct EndLeftHand;
        public Vector2Struct StartRightHand;
        public Vector2Struct EndRightHand;
        public ZoomGestureEventArgs(Vector2Struct startLeftHand,Vector2Struct endLeftHand, Vector2Struct startRightHand, Vector2Struct endRightHand)
        {
            StartLeftHand = startLeftHand;
            EndLeftHand = endLeftHand;
            StartRightHand = startRightHand;
            EndRightHand = endRightHand;
        }
    }
    public class ZoomGestureDetector : GestureDetector
    {
        public float SwipeMinimalLength {get;set;}
        public float SwipeMaximalHeight {get;set;}
        public int ZoomTriggerDuration {get;set;}
        public int SwipeMaximalDuration { get; set; }

        public float DiffMinimalZLenth { get; set; }


        private double leftX;
        private double rightX;
        private bool trackStarted = false;
        private double YBetweenHandAndElbow = 0.1;
        private double YDiffThresh = 0.1;
        private double ZBetweenHandAndElbow = 0.1;
        private double ZDiffThresh = 0.1;

        public ZoomGestureDetector(int windowSize = 40)
            : base(windowSize)
        {
            SwipeMinimalLength = 0.4f;
            DiffMinimalZLenth = 0.15f;
            SwipeMaximalHeight = 0.2f;
            ZoomTriggerDuration = 250;
            SwipeMaximalDuration = 1500;
        }

        protected bool ScanPositions(Func<Vector3Struct, Vector3Struct, bool> heightFunction, Func<Vector3Struct, Vector3Struct, bool> directionFunction, 
            Func<Vector3Struct, Vector3Struct, bool> lengthFunction, int minTime, int maxTime)
        {
            int start = 0;

            for (int index = 1; index < Entries.Count - 1; index++)
            {
                Skeleton firstSkeleton = Entries[0].Skeleton;
                SkeletonPoint firstLeftHand = firstSkeleton.Joints[JointType.HandLeft].Position,
                    firstRightHand = firstSkeleton.Joints[JointType.HandRight].Position;
                Vector3Struct firstLeftVec3 = firstLeftHand.ToVector3(),
                    firstRightVec3 = firstRightHand.ToVector3();

                Skeleton scanSkeleton = Entries[index].Skeleton;
                Vector3Struct scanLeftVec3 = scanSkeleton.Joints[JointType.HandLeft].Position.ToVector3(),
                    scanRightVec3 = scanSkeleton.Joints[JointType.HandRight].Position.ToVector3();
                Skeleton nextScanSkeleton = Entries[index+1].Skeleton;
                Vector3Struct nextScanLeftVec3 = nextScanSkeleton.Joints[JointType.HandLeft].Position.ToVector3();
                if (!heightFunction(firstLeftVec3, scanLeftVec3) || !directionFunction(scanLeftVec3, nextScanLeftVec3))
                {
                    start = index;
                }

                if (lengthFunction(scanLeftVec3, firstLeftVec3))
                {
                    double totalMilliseconds = (Entries[index].Time - Entries[start].Time).TotalMilliseconds;
                    if (totalMilliseconds >= minTime && totalMilliseconds <= maxTime)
                    {
                        return true;
                    }
                }
            }

            return false;
        }

        private float len = 0.0f;
        private bool IsZooming = false;
        protected void LookForGesture_bak()
        {
            int pos = 0, index = 0;
            float avgDistance = 0.0f;

            for (pos = 0; pos < Entries.Count;pos++ )
            {

                Skeleton posSkeleton = Entries[pos].Skeleton;
                SkeletonPoint posLeftHand = posSkeleton.Joints[JointType.HandLeft].Position,
                    posRightHand = posSkeleton.Joints[JointType.HandRight].Position,
                    posHipCenter = posSkeleton.Joints[JointType.HipCenter].Position;
                Vector3Struct oneHand = posLeftHand.ToVector3(),
                    otherHand = posRightHand.ToVector3(),
                    hipPoint = posHipCenter.ToVector3(); ;

 
                // 过滤不在身体前面小范围的点
                if ( Math.Abs(oneHand.Z - otherHand.Z) > DiffMinimalZLenth
                    || (oneHand.Z - hipPoint.Z > -0.2f || otherHand.Z-hipPoint.Z > -0.2f)
                    || (oneHand.Y - hipPoint.Y < 0.2f || otherHand.Y - hipPoint.Y < 0.2f)
                    || (Math.Abs(oneHand.X - hipPoint.X) > 0.2f || Math.Abs(otherHand.X - hipPoint.X) > 0.2f))
                {
                    
                    index = pos;
                    len = (oneHand - otherHand).Length;
                    continue;
                }
                double duringTotalTime = (Entries[pos].Time - Entries[index].Time).TotalMilliseconds;
                System.Diagnostics.Debug.WriteLine(String.Format("{0} {1} {2}",pos,index,duringTotalTime));
                if (duringTotalTime > ZoomTriggerDuration)
                {
                    float distance = (oneHand - otherHand).Length;
                    if (avgDistance == 0.0f)
                        avgDistance = distance;
                    else
                        avgDistance = (distance + avgDistance)/2;
                    IsZooming = true;
                }
                else if (IsZooming)
                {
                    float distance = (oneHand - otherHand).Length;
                    avgDistance = distance;
                }

            }

            if ( IsZooming )
            {
                float factor = avgDistance / len;
                GestureEventArgs zoomGesture = new GestureEventArgs(E_GestureDescrition.ZoomIn,JointType.HandLeft&JointType.HandRight);
                zoomGesture.ScaleFactor = factor;
                //RaiseGestureDetected(zoomGesture);
            }
        }


        protected override void LookForGesture()
        {
            bool flag = false;
            if ( !this.trackStarted )
            {
                int count = 0;
                int pos = 0;
                Skeleton skeleton = null ;
                for ( pos = 0; pos < Entries.Count; pos++)
                {
                    skeleton = Entries[pos].Skeleton;
                    flag = this.IsInPosture(skeleton);
                    
                    if (flag)
                    {
                        count++;
                        if (count>5)
                            break;
                    }
                }

                if (count > 0)
                {
                    Joint joint;
                    joint = skeleton.Joints[JointType.HandRight];
                    this.rightX = joint.Position.X;
                    joint = skeleton.Joints[JointType.HandLeft];
                    this.leftX = joint.Position.X;
                    this.trackStarted = true;

                    GestureEventArgs args = new GestureEventArgs(E_GestureDescrition.ZoomIn, JointType.HandLeft);
                    args.ScaleFactor = 1.0;
                    RaiseGestureDetected(args);
                }
            }
            else if (Entries.Count>0)
            {
                Skeleton skeleton = Entries[Entries.Count-1].Skeleton;
                flag = this.IsInPosture(skeleton);
                if (flag)
                {
                    Joint joint;
                    double num = this.rightX - this.leftX;

                    double num2 = skeleton.Joints[JointType.HandRight].Position.X - skeleton.Joints[JointType.HandLeft].Position.X;

                    GestureEventArgs args = new GestureEventArgs(E_GestureDescrition.ZoomIn, JointType.HandLeft);
                    args.ScaleFactor = num2 / num;

                    RaiseGestureDetected(args);
                    System.Diagnostics.Debug.WriteLine(String.Format("Zoom:{0}", args.ScaleFactor));
                    joint = skeleton.Joints[JointType.HandRight];
                    this.rightX = joint.Position.X;
                    joint = skeleton.Joints[JointType.HandLeft];
                    this.leftX = joint.Position.X;
                }
                else
                {
                    this.trackStarted = false;
                }
            }


        }


        private bool IsInPosture(Skeleton skeleton)
        {
            Joint joint = skeleton.Joints[JointType.HandLeft];
            SkeletonPoint position = joint.Position;
            joint = skeleton.Joints[JointType.HandRight];
            SkeletonPoint point2 = joint.Position;
            joint = skeleton.Joints[JointType.ElbowLeft];
            SkeletonPoint point3 = joint.Position;
            joint = skeleton.Joints[JointType.ElbowRight];
            SkeletonPoint point4 = joint.Position;
            if (Math.Abs((float)(position.Z - point2.Z)) > this.ZDiffThresh)
            {
                //Console.WriteLine("zdiff:" + Math.Abs((float)(position.Z - point2.Z)));
                return false;
            }
            if (Math.Abs((float)(position.Y - point2.Y)) > this.YDiffThresh)
            {
                //Console.WriteLine("ydiff:" + Math.Abs((float)(position.Y - point2.Y)));
                return false;
            }
            if ((position.Y - point3.Y) < this.YBetweenHandAndElbow)
            {
                //Console.WriteLine("YBetweenHandAndElbowleft:" + (position.Y - point3.Y));
                return false;
            }
            if ((point2.Y - point4.Y) < this.YBetweenHandAndElbow)
            {
                //Console.WriteLine("YBetweenHandAndElbowright:" + (point2.Y - point4.Y));
                return false;
            }
            if ((point3.Z - position.Z) < this.ZBetweenHandAndElbow)
            {
                //Console.WriteLine("ZBetweenHandAndElbowleft:" + (point3.Z - position.Z));
                return false;
            }
            if ((point4.Z - point2.Z) < this.ZBetweenHandAndElbow)
            {
                //Console.WriteLine("ZBetweenHandAndElbowright:" + (point4.Z - point2.Z));
                return false;
            }
            return true;
        }



    }
}
